/*! @file MiniCheetah.h
 *  @brief Utility function to build a Mini Cheetah Quadruped object
 *
 * This file is based on MiniCheetahFullRotorModel_mex.m and builds a model
 * of the Mini Cheetah robot.  The inertia parameters of all bodies are
 * determined from CAD.
 *
 */

#ifndef PROJECT_MINICHEETAH_H
#define PROJECT_MINICHEETAH_H

#include "FloatingBaseModel.h"
#include "Quadruped.h"

/*!
 * Generate a Quadruped model of Mini Cheetah
 */
template <typename T>
Quadruped<T> buildMiniCheetah()
{
    Quadruped<T> cheetah;
    cheetah._robotType = RobotType::MINI_CHEETAH;// 设置机器人类型为迷你猎豹

    // 身体物理参数
    cheetah._bodyMass   = 4.5;      // 身体质量 (kg) 3.3
    cheetah._bodyLength = 0.175 * 2;// 身体长度 (m)，乘以2是因为原参数为半长 0.19 * 2
    cheetah._bodyWidth  = 0.048 * 2;// 身体宽度 (m)，乘以2是因为原参数为半宽 0.049 * 2
    cheetah._bodyHeight = 0.055 * 2;// 身体高度 (m)，乘以2是因为原参数为半高 0.05 * 2

    // 关节齿轮比
    cheetah._abadGearRatio = 7.75;// ABAD关节齿轮比 6
    cheetah._hipGearRatio  = 7.75;// 髋关节齿轮比 6
    cheetah._kneeGearRatio = 7.75;// 膝关节齿轮比 9.33

    // 腿部连杆长度参数
    cheetah._abadLinkLength   = 0.099;// ABAD连杆长度 (m) 0.062
    cheetah._hipLinkLength    = 0.15;// 髋关节连杆长度 (m) 0.209
    cheetah._kneeLinkY_offset = 0;    // 膝关节连杆Y轴偏移量 (m) 0.004
    cheetah._kneeLinkLength   = 0.15; // 膝关节连杆长度 (m) 0.195
    cheetah._maxLegLength     = 0.283;  // 最大腿长 (m)，由连杆长度计算得出 0.409

    // 电机与关节参数
    cheetah._motorTauMax      = 17.f; // 电机最大输出扭矩 (Nm) 3.f
    cheetah._batteryV         = 24;   // 电池电压 (V) 24
    cheetah._motorKT          = 1.22; // 电机扭矩常数 (Nm/A)，等于磁链乘极对数 0.05
    cheetah._motorR           = 0.173;// 电机内阻 (Ω) 0.173
    cheetah._jointDamping     = .01;  // 关节阻尼系数 0.01
    cheetah._jointDryFriction = .2;   // 关节干摩擦系数 0.2

    // 转子惯性参数（绕Z轴旋转时的转动惯量矩阵）
    Mat3<T> rotorRotationalInertiaZ;
    rotorRotationalInertiaZ << 2.2, 0, 0, 0, 2.2, 0, 0, 0, 4.2;// 转动惯量分量 (kg·m²×1e-3) 33, 0, 0, 0, 33, 0, 0, 0, 63;
    rotorRotationalInertiaZ = 1e-3 * rotorRotationalInertiaZ;  // 缩放至实际单位 1e-6

    // 旋转矩阵：用于将惯性张量从Z轴方向转换到X/Y轴方向
    Mat3<T> RY                      = coordinateRotation<T>(CoordinateAxis::Y, M_PI / 2);// Y轴旋转90度
    Mat3<T> RX                      = coordinateRotation<T>(CoordinateAxis::X, M_PI / 2);// X轴旋转90度
    Mat3<T> rotorRotationalInertiaX = RY * rotorRotationalInertiaZ * RY.transpose();     // 绕X轴转子惯量
    Mat3<T> rotorRotationalInertiaY = RX * rotorRotationalInertiaZ * RX.transpose();     // 绕Y轴转子惯量

    // 空间惯性参数（基于CAD数据）
    // ABAD部件惯性
    Mat3<T> abadRotationalInertia;
    abadRotationalInertia << 106.22, 0.13, 0.07, 0.13, 163.78, 0.04, 0.07, 0.04, 106.1;// 转动惯量矩阵 (kg·m²×1e-6) 381, 58, 0.45, 58, 560, 0.95, 0.45, 0.95, 444
    abadRotationalInertia = abadRotationalInertia * 1e-6;                              // 缩放至实际单位
    Vec3<T>           abadCOM(0.036, -0.008, 0);                                       // ABAD部件质心位置 (m，相对于关节坐标系) 0, 0.036, 0
    SpatialInertia<T> abadInertia(0.15, abadCOM, abadRotationalInertia);               // ABAD空间惯性 (质量kg, 质心, 转动惯量) 0.54

    // 髋关节部件惯性
    Mat3<T> hipRotationalInertia;
    hipRotationalInertia << 4749.56, 11.96, -90.13, 11.96, 4881.08, 696.15, 90.13, 696.15, 937.95;// 转动惯量矩阵 (kg·m²×1e-6) 1983, 245, 13, 245, 2103, 1.5, 13, 1.5, 408
    hipRotationalInertia = hipRotationalInertia * 1e-6;                                           // 缩放至实际单位
    Vec3<T>           hipCOM(-0.017, -0.052, 0.009);                                              // 髋关节部件质心位置 (m) 0, 0.016, -0.02
    SpatialInertia<T> hipInertia(1.1, hipCOM, hipRotationalInertia);                              // 髋关节空间惯性 0.634

    // 膝关节部件惯性
    Mat3<T> kneeRotationalInertia, kneeRotationalInertiaRotated;
    kneeRotationalInertiaRotated << 367.8, 0, -47.42, 0, 379.33, 0, -47.42, 0, 16.67; // 旋转后膝关节转动惯量 (kg·m²×1e-6) 6, 0, 0, 0, 248, 0, 0, 0, 245
    kneeRotationalInertiaRotated = kneeRotationalInertiaRotated * 1e-6;               // 缩放至实际单位
    kneeRotationalInertia        = RY * kneeRotationalInertiaRotated * RY.transpose();// 转换到关节坐标系
    Vec3<T>           kneeCOM(0.073, -0.009, -0.008);                                 // 膝关节部件质心位置 (m) 0, 0, -0.061
    SpatialInertia<T> kneeInertia(0.05, kneeCOM, kneeRotationalInertia);              // 膝关节空间惯性 0.064

    // 转子惯性（电机转子）
    Vec3<T>           rotorCOM(0, 0, 0);                                      // 转子质心位置 (m)
    SpatialInertia<T> rotorInertiaX(0.095, rotorCOM, rotorRotationalInertiaX);// X轴转子空间惯性 0.055
    SpatialInertia<T> rotorInertiaY(0.095, rotorCOM, rotorRotationalInertiaY);// Y轴转子空间惯性 0.055

    // 身体惯性
    Mat3<T> bodyRotationalInertia;
    bodyRotationalInertia << 11253, 0, 0, 0, 36203, 0, 0, 0, 42673;                  // 身体转动惯量矩阵 (kg·m²×1e-6) 11253, 0, 0, 0, 36203, 0, 0, 0, 42673
    bodyRotationalInertia = bodyRotationalInertia * 1e-6;                            // 缩放至实际单位
    Vec3<T>           bodyCOM(0, 0, 0);                                              // 身体质心位置 (m，位于身体坐标系原点)
    SpatialInertia<T> bodyInertia(cheetah._bodyMass, bodyCOM, bodyRotationalInertia);// 身体空间惯性

    // 将惯性参数赋值给机器人模型
    cheetah._abadInertia      = abadInertia;
    cheetah._hipInertia       = hipInertia;
    cheetah._kneeInertia      = kneeInertia;
    cheetah._abadRotorInertia = rotorInertiaX;
    cheetah._hipRotorInertia  = rotorInertiaY;
    cheetah._kneeRotorInertia = rotorInertiaY;
    cheetah._bodyInertia      = bodyInertia;

    // 部件位置参数（相对于各自关节坐标系）
    cheetah._abadRotorLocation = Vec3<T>(cheetah._bodyLength, cheetah._bodyWidth, 0) * 0.5;// ABAD转子位置 (m) 0.125, 0.049, 0 * 1
    cheetah._abadLocation      = Vec3<T>(cheetah._bodyLength, cheetah._bodyWidth, 0) * 0.5;// ABAD关节位置 (m)，位于身体边缘
    cheetah._hipLocation       = Vec3<T>(0, cheetah._abadLinkLength, 0);                   // 髋关节位置 (m)，沿abad连杆方向
    cheetah._hipRotorLocation  = Vec3<T>(0, 0.04, 0);                                      // 髋关节转子位置 (m) 0, 0.04, 0
    cheetah._kneeLocation      = Vec3<T>(0, 0, -cheetah._hipLinkLength);                   // 膝关节位置 (m)，沿髋关节连杆负方向
    cheetah._kneeRotorLocation = Vec3<T>(0, 0, 0);                                         // 膝关节转子位置 (m)

    return cheetah;// 返回构建完成的Mini Cheetah机器人模型
}

#endif// PROJECT_MINICHEETAH_H
